#include "Vofa_printf.h"
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>

// JustFloat协议尾帧
static const uint8_t FOOTER[4] = {0x00, 0x00, 0x80, 0x7F};

/* 解析格式字符串中%f的数量 */
static int count_float_specifiers(const char* fmt)
{
    int count = 0;
    while(*fmt)
    {
        if(*fmt == '%')
        {
            if(*(fmt + 1) == 'f')
            {  // 发现%f
                count++;
                fmt += 2;
                continue;
            }
            else if(*(fmt + 1) == '%')
            {  // 处理转义%%
                fmt += 2;
                continue;
            }
        }
        fmt++;
    }
    return count;
}

void VOFA_printf_justfloat(const char* fmt, ...)
{
    va_list args;
    va_start(args, fmt);

    // 计算需要的浮点数数量
    int num_floats = count_float_specifiers(fmt);
    if(num_floats == 0)
    {
        // 发送空数据+尾帧
        CDC_Transmit_FS((uint8_t*)FOOTER, sizeof(FOOTER));
        va_end(args);
        return;
    }

    // 动态分配内存：数据区 + 尾帧
    size_t data_size = num_floats * sizeof(float);
    uint8_t* buffer = malloc(data_size + sizeof(FOOTER));
    if(!buffer)
    {
        va_end(args);
        return;
    }

    // 填充浮点数据
    float* float_ptr = (float*)buffer;
    for(int i = 0; i < num_floats; i++)
    {
        *float_ptr++ = (float)va_arg(args, double);
    }

    // 添加协议尾帧
    memcpy(buffer + data_size, FOOTER, sizeof(FOOTER));

    // 发送完整数据包
    CDC_Transmit_FS(buffer, data_size + sizeof(FOOTER));

    // 清理资源
    free(buffer);
    va_end(args);
}
